import numpy as np
import pandas as pd
dic1 = {
    (1, 1): 45,
    (1, 0): 90,
    (1, -1): 135,
    (0, -1): 180,
    (-1, -1): 225,
    (-1, 0): 270,
    (-1, 1): 315,
    (0, 1): 0,
}

def read():
    file_path = '附件7：P5-P6行驶路径.xlsx'
    df = pd.read_excel(file_path)
    array = df[['编号','栅格x坐标', '栅格y坐标']].values.tolist()
    return array

path=read()
ans=0
print(0,":",ans)
for i in range(len(path)-2):
    x, y = path[i][1], path[i][2]
    xn, yn = path[i+1][1], path[i+1][2]
    xnn, ynn = path[i+2][1], path[i+2][2]
    dP =(xn-x,yn-y)
    dPn=(xnn-xn,ynn-yn)
    dtheta=abs(dic1[dP]-dic1[dPn])
    dtheta=min(dtheta,360-dtheta)
    ans+= (np.sqrt(abs(dP[0])+abs(dP[1]))+dtheta/90)*5
    print(i,":",ans)

ans+= np.sqrt(abs(dPn[0])+abs(dPn[1]))*5
print(i+1,":",ans)
